{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('../scripts/')\n",
    "from mcl import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ResetMcl(Mcl): ###resetmcl4 （random_reset, sensor_resetting_drawは省略）\n",
    "    def __init__(self, envmap, init_pose, num, motion_noise_stds={\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2}, \\\n",
    "                 distance_dev_rate=0.14, direction_dev=0.05, amcl_params={\"slow\":0.001, \"fast\":0.1, \"nu\":3.0}): #amcl_paramsを追加\n",
    "        super().__init__(envmap, init_pose, num, motion_noise_stds, distance_dev_rate, direction_dev)\n",
    "        self.amcl_params = amcl_params #追加\n",
    "        self.slow_term_alpha, self.fast_term_alpha = 1.0, 1.0 #追加\n",
    "        \n",
    "    def random_reset(self):\n",
    "         for p in self.particles:\n",
    "                p.pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "                p.weight = 1/len(self.particles)\n",
    "                \n",
    "    def sensor_resetting_draw(self, particle, landmark_pos, ell_obs, phi_obs):\n",
    "            ##パーティクルの位置を決める##\n",
    "            psi = np.random.uniform(-np.pi, np.pi) #ランドマークからの方角を選ぶ\n",
    "            ell = norm(loc=ell_obs, scale=(ell_obs*self.distance_dev_rate)**2).rvs() #ランドマークからの距離を選ぶ\n",
    "            particle.pose[0] = landmark_pos[0] + ell*math.cos(psi)\n",
    "            particle.pose[1] = landmark_pos[1] + ell*math.sin(psi)\n",
    "            \n",
    "            ##パーティクルの向きを決める##\n",
    "            phi = norm(loc=phi_obs, scale=(self.direction_dev)**2).rvs() #ランドマークが見える向きを決める\n",
    "            particle.pose[2] = math.atan2(landmark_pos[1]- particle.pose[1], landmark_pos[0]- particle.pose[0]) - phi\n",
    "            \n",
    "            particle.weight = 1.0/len(self.particles)\n",
    "            \n",
    "    def sensor_resetting(self, observation):\n",
    "        nearest_obs = np.argmin([obs[0][0] for obs in observation])\n",
    "        values, landmark_id = observation[nearest_obs]\n",
    "        \n",
    "        for p in self.particles:\n",
    "            self.sensor_resetting_draw(p, self.map.landmarks[landmark_id].pos, *values)\n",
    "            \n",
    "    def adaptive_resetting(self, observation):\n",
    "        if len(observation) == 0: return #追加\n",
    "        \n",
    "        #＃センサリセットするパーティクルの数を決める##\n",
    "        alpha = sum([p.weight for p in self.particles])\n",
    "        self.slow_term_alpha += self.amcl_params[\"slow\"]*(alpha - self.slow_term_alpha)\n",
    "        self.fast_term_alpha += self.amcl_params[\"fast\"]*(alpha - self.fast_term_alpha)\n",
    "        sl_num = len(self.particles)*max([0, 1.0-self.amcl_params[\"nu\"]*self.fast_term_alpha/self.slow_term_alpha])\n",
    "        \n",
    "        self.resampling() #とりあえず普通にリサンプリング\n",
    "        \n",
    "        nearest_obs = np.argmin([obs[0][0] for obs in observation]) #距離が一番近いランドマークを選択\n",
    "        values, landmark_id = observation[nearest_obs]\n",
    "        for n in range(int(sl_num)): #n回パーティクルを選んで姿勢を変える（2回以上姿勢を変えられるパーティクルがあるけどとりあえず気にしない）\n",
    "            p = random.choices(self.particles)[0] #一つ選ぶ\n",
    "            self.sensor_resetting_draw(p, self.map.landmarks[landmark_id].pos, *values)\n",
    "        \n",
    "    def observation_update(self, observation):\n",
    "        for p in self.particles:\n",
    "            p.observation_update(observation, self.map, self.distance_dev_rate, self.direction_dev) \n",
    "    \n",
    "        self.set_ml()\n",
    "        self.adaptive_resetting(observation) #変更"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial(animation): ###mclkidnap1test\n",
    "    time_interval = 0.1\n",
    "    world = World(300, time_interval, debug=not animation) \n",
    "\n",
    "    ## 地図を生成して3つランドマークを追加 ##\n",
    "    m = Map()\n",
    "    m.append_landmark(Landmark(-4,2))\n",
    "    m.append_landmark(Landmark(2,-3))\n",
    "    m.append_landmark(Landmark(3,3))\n",
    "    world.append(m)\n",
    "\n",
    "    ## ロボットを作る ##\n",
    "    init_pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "    robot_pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "    pf = ResetMcl(m, init_pose, 300)\n",
    "    a = EstimatorAgent(time_interval, 0.2, 10.0/180*math.pi, pf)\n",
    "    r = Robot(robot_pose, sensor=Camera(m, phantom_prob=0.1), agent=a, color=\"red\")\n",
    "    world.append(r)\n",
    "\n",
    "    world.draw()\n",
    "    \n",
    "    return (pf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf = trial(True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.log\n",
    "long = [e[0] for e in pf.log]\n",
    "short = [e[1] for e in pf.log]\n",
    "s_l = [e[1]/e[0] for e in pf.log]\n",
    "#plt.hist(s_l)\n",
    "#plt.show()\n",
    "min(s_l), max(s_l)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "min(s_l), max(s_l)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def trial_phantom(animation): ###mclkidnap1test\n",
    "    time_interval = 0.1\n",
    "    world = World(300, time_interval, debug=not animation) \n",
    "\n",
    "    ## 地図を生成して3つランドマークを追加 ##\n",
    "    m = Map()\n",
    "    m.append_landmark(Landmark(-4,2))\n",
    "    m.append_landmark(Landmark(2,-3))\n",
    "    m.append_landmark(Landmark(3,3))\n",
    "    world.append(m)\n",
    "\n",
    "    ## ロボットを作る ##\n",
    "    init_pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "    robot_pose = np.array([np.random.uniform(-5.0, 5.0), np.random.uniform(-5.0, 5.0), np.random.uniform(-math.pi, math.pi)]).T\n",
    "    pf = ResetMcl(m, init_pose, 1000)\n",
    "    a = EstimatorAgent(time_interval, 0.2, 10.0/180*math.pi, pf)\n",
    "    r = Robot(robot_pose, sensor=Camera(m, phantom_prob=0.1), agent=a, color=\"red\")\n",
    "    world.append(r)\n",
    "\n",
    "    world.draw()\n",
    "    \n",
    "    return (pf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf = trial_phantom(True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.long_term_alpha"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pf.short_term_alpha"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
